#include <Arduino.h>
#include <WiFi.h>
#include "src/Handler.h"
/*****************************************************
* Hardware: ESP32 Devkit V1-30PINS
* Version: 1.0.0.0
* Auther: Reser
* Date: 2020-12-21
*
* Serial:
* 	Serial0: UART
* 	Serial1: MP3
* 	Serial2: Servo bus
******************************************************/


WiFiServer g_server;
WiFiClient g_client;
IPAddress AP_IP(192, 168, 4, 1);
IPAddress AP_GATEWAY(192, 168, 4, 1);
IPAddress AP_MASK(255, 255, 255, 0);

HardwareSerial g_serial1(1);
CRingBuffer g_buffer;
CSettings g_setting;
CServoManager g_servos;
CKT404A g_kt404a;
CHandler g_handler;


/////////////////////////////////////////////////////////////
// Time-consuming tasks...
void getActionsTask()
{
	File root;
	do {
		root = SPIFFS.open("/");
		if (!root) {
			LOG("Open SPIFFS error");
			break;
		}

		LOG("Action send begin...");
		uint8_t snd[256];
		uint32_t size = 256;
		File file = root.openNextFile();
		while(file)
		{
			if (!file.isDirectory()) {
				String name(file.name());
				name = "/" + name;
				AeActHeader_t header;
				if (!CActionOpt::ReadHeader(name, header)) {
					// Error, next file...
					LOG("Removing invalid file: " + name);
					SPIFFS.remove(name);
					file = root.openNextFile();
					continue;
				}

				size = 256;
				CProtoSerial::PackGetactSending(size, snd, name.length(), name.c_str(), header);
				if (g_client.connected()) {
					g_client.write(snd, size);
				}
				LOG("Sending action-" + name);
			}

			// Next file...
			file = root.openNextFile();
		}

		size = 256;
		CProtoSerial::PackGetactFinished(size, snd);
		if (g_client.connected()) {
			g_client.write(snd, size);
		}
		LOG("Action sending done");
	} while(false);
	root.close();
}

void setAngleTask()
{
	g_servos.SetAngle(g_setting.m_set.idx-1, g_setting.m_set.angle, g_setting.m_set.timeMsec);
}

void setAnglesTask()
{
	for (int i = 0;i < g_setting.m_set.servoCnt;i++) {
		g_servos.SetAngle(i, g_setting.m_set.angles[i], g_setting.m_set.timeMsec);
	}
}

void processCmds()
{
	// Loop to process cmds in buffer...
  while (g_buffer.Available() > 2)
  {
    uint8_t c0 = g_buffer.Peek(0);
    uint8_t c1 = g_buffer.Peek(1);

    if ((c0 == 0xFA && c1 == 0xAF) || (c0 == 0xFC && c1 == 0xCF)) {
			// UBT raw servo cmd

      if (g_buffer.Available() < UBT_SERVO_CMD_SIZE) {
        // Not completed, pass to next processing...
        break;
      }

      uint8_t snd[UBT_SERVO_CMD_SIZE];
      uint8_t rcv[UBT_SERVO_CMD_SIZE];
      g_buffer.Get(UBT_SERVO_CMD_SIZE, snd);
      if (snd[UBT_SERVO_CMD_SIZE - 1] != 0xED) {
        // Skip to find next cmd header...
        g_buffer.Skip();
        continue;
      }

      LOG("Handle UBT raw servo cmd...");
      int expect = 0;
      if (SERVO_RET_SUCCESS == CUBTServoExec::Execute(&Serial2, snd, expect, rcv)) {
        if (g_client.connected()) {
          g_client.write(rcv, expect);
        }
      }
    }
    else if (c0 == 0xFB && c1 == 0xBF) {
			// Custom handler cmd
      
			uint8_t len = g_buffer.Peek(2);
      if (g_buffer.Available() < len) {
        // Not completed, pass to next processing...
        break;
      }

      uint8_t snd[256];
      uint8_t rcv[256];
      g_buffer.Get(len, snd);
      if (snd[len - 1] != 0xED) {
        // Skip to find next cmd header...
        g_buffer.Skip();
        continue;
      }

      LOG("Handle custom handler cmd");
      int expect = 0;
      AeRet_t ret = g_handler.HandleCmd(len, snd, expect, rcv);
      if (ret < AE_RET_ERROR) {
        if (g_client.connected()) {
          g_client.write(rcv, expect);
        }

        // Tasks need more time to execute...
        switch (ret)
        {
        case AE_RET_GET_ACTIONS_TASK:
          getActionsTask();
          break;
				case AE_RET_SET_ANGLE_TASK:
					setAngleTask();
					break;
				case AE_RET_SET_ANGLES_TASK:
					setAnglesTask();
					break;
        default:
          break;
        }
      }
    }
    else {
      // Skip to find next cmd header...
      g_buffer.Skip();
    }
  }
}

void setIdel()
{
  g_setting.m_set.state = AE_STS_IDEL;
	g_setting.m_set.action = "";
  g_setting.m_set.seqCnt = 0;
  g_setting.m_set.nextSeq = 0;
  g_setting.m_set.servoMsec = 0;
  g_setting.m_set.waitMsec = 0;
  g_setting.m_set.startMsec = 0;
}

void scheduleTasks()
{
	bool error = false;
	switch (g_setting.m_set.state)
	{
	case AE_STS_IDEL:
		break;
	case AE_STS_PLAYING:
	{
		// Process only when action time reached
		if (millis() >= (g_setting.m_set.startMsec + g_setting.m_set.waitMsec)) {
			if (g_setting.m_set.nextSeq >= g_setting.m_set.seqCnt) {
				// Finished
				LOG("Playing action finished");

				g_kt404a.Stop();
#ifdef AE_EYE_PIN
				digitalWrite(AE_EYE_PIN, LOW);
#endif

				setIdel();

				// Notify...
				uint8_t snd[256];
				uint32_t size = 256;
				CProtoSerial::PackPlayactFinished(size, snd);
				if (g_client.connected()) {
					g_client.write(snd, size);
				}
			}
			else {
				// Start next pose
				LOG("Playing pose-" + String(g_setting.m_set.nextSeq));

				if (g_setting.m_set.action.length() == 0) {
					error = true;
					break;
				}
				AeActPose_t pose;
				if (!CActionOpt::ReadPose(g_setting.m_set.action, g_setting.m_set.nextSeq, pose)) {
					error = true;
					break;
				}

				g_setting.m_set.servoMsec = pose.servoMsec;
				g_setting.m_set.waitMsec = pose.waitMsec;
				g_setting.m_set.startMsec = millis();
				g_setting.m_set.nextSeq++;

				for (int i = 0; i < SERVO_MAX_CNT;i++) {
					g_servos.SetAngle(i, pose.angles[i], pose.servoMsec);
				}
#ifdef AE_EYE_PIN
				digitalWrite(AE_EYE_PIN, 1 == pose.led ? HIGH : LOW);
#endif

				// Notify...
				uint8_t snd[256];
				uint32_t size = 256;
				CProtoSerial::PackPlayactSending(size, snd, g_setting.m_set.seqCnt, g_setting.m_set.nextSeq);
				if (g_client.connected()) {
					g_client.write(snd, size);
				}
			}
		}
	}
		break;
	case AE_STS_PAUSED:
		break;
	case AE_STS_TEACHING:
  {
    // Process only when action time reached
    if (millis() >= (g_setting.m_set.startMsec + g_setting.m_set.waitMsec)) {
      LOG("Teaching pose-" + String(g_setting.m_set.seqCnt));

      if (g_setting.m_set.action.length() == 0) {
				error = true;
				break;
			}
			AeActPose_t pose;
      pose.seq = g_setting.m_set.seqCnt;
      pose.servoMsec = g_setting.m_set.servoMsec;
	    pose.waitMsec = g_setting.m_set.waitMsec;
	    pose.led = 1;
      for (int i = 0;i < AE_SERVO_MAX;i++) {
        pose.angles[i] = 0;
        if (SERVO_RET_SUCCESS != g_servos.GetAngle(i, pose.angles[i])) {
          pose.angles[i] = g_setting.m_set.lastAngles[i];
        }
        g_setting.m_set.lastAngles[i] = pose.angles[i];
      }
			if (!CActionOpt::WritePose(g_setting.m_set.action, pose.seq, pose)) {
				error = true;
				break;
			}

			g_setting.m_set.seqCnt++;
			g_setting.m_set.startMsec = millis();

			// Notify...
			uint8_t snd[256];
			uint32_t size = 256;
			CProtoSerial::PackTeachSending(size, snd, pose.angles);
			if (g_client.connected()) {
				g_client.write(snd, size);
			}
    }
  }
		break;
	default:
		break;
	}

  // Error occurs
  if (error) {
    LOG("Error occurs, recovering...");

    g_kt404a.Stop();
#ifdef AE_EYE_PIN
    digitalWrite(AE_EYE_PIN, LOW);
#endif
    setIdel();
  }
}

void setup() {
  // put your setup code here, to run once:
	// Serial config
	Serial.begin(115200);
	g_serial1.begin(9600, SERIAL_8N1, 33, 32); // RX TX
	Serial2.begin(115200);

	LOG("*********************** RobotFirmware V2.1 ********************");

	// Miscs config
	LOG("Miscs config...");
#ifdef AE_EYE_PIN
	pinMode(AE_EYE_PIN, OUTPUT);
	digitalWrite(AE_EYE_PIN, LOW);
#endif
#ifdef TXEN_PIN
	pinMode(TXEN_PIN, OUTPUT);
	digitalWrite(TXEN_PIN, HIGH);
#endif
	if (!g_buffer.Create(AE_RCV_BUFFER_SIZE)) {
		LOG("Buffer allocate error");
	}
	LOG("Miscs config done");

	// Settings config
	LOG("Settings config...");
	g_setting.Reset();
	LOG("Settings config done");

	// Servos config
	LOG("Servos config...");
	uint16_t initAngles[] = {90, 90, 90, 90, 90, 90, 90, 60, 76, 110, 90, 90, 120, 104, 70, 90};
	int ids[] = {1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16};
	g_servos.Init(SERVO_UBT, g_setting.m_set.servoCnt, ids, initAngles,
		g_setting.m_set.offsetMin, g_setting.m_set.offsetMax, &Serial2);
	LOG("Servos config done");

	// MP3
	LOG("MP3 config...");
	g_kt404a.Init(&g_serial1);
	LOG("MP3 config done");

	// SPIFFS
	LOG("SPIFFS config...");
	if (!SPIFFS.begin(true)) {
		LOG("SPIFFS mount error");
	}
	LOG("SPIFFS config done");

	// AP config
	LOG("AP config...");
	if (!WiFi.mode(WIFI_AP)) {
		LOG("AP mode error");
	}
	delay(2000);
	//if (!WiFi.softAPConfig(AP_IP, AP_GATEWAY, AP_MASK)) {
	//	LOG("AP params error");
	//}
	if (!WiFi.softAP(g_setting.m_set.name.c_str(), g_setting.m_set.apPwd.c_str())) {
		LOG("AP setup error");
	}
	delay(1000);
	WiFi.softAPsetHostname(g_setting.m_set.name.c_str());
	LOG("AP config done");

	// TCP/IP config
	LOG("TCP/IP config...");
	g_server.begin(g_setting.m_set.apPort);
	g_server.setTimeout(1800);
	g_server.setNoDelay(true);
	delay(1000);
	LOG("TCP/IP config done");

	// Init pose
	g_servos.SetToInitAngles();

	LOG("************** Config Done ***************");
}

void loop() {
  // put your main code here, to run repeatedly:
	// Loop to accept client
	if (g_server.hasClient()) {
		g_client = g_server.accept();
		LOG("Accepted one client");

		// Loop to communicate...
		while (g_client.connected())
		{
			int avail = g_client.available();
			if (avail > 0) {
				// Rcv data and push into buffer
				int toRcv = min(avail, g_buffer.Free());
				uint8_t buf[AE_RCV_BUFFER_SIZE];
				g_client.readBytes(buf, toRcv);
				g_buffer.Put(toRcv, buf);

				// Process cmds
				processCmds();

				// Schedule tasks
				scheduleTasks();
			}
			else {
				// Schedule tasks
				scheduleTasks();

				delay(2);
			}
		}

		LOG("Free one client");
		g_client.stop();
		g_buffer.Reset();
	}

	delay(20);
}
